//
// Created by yunnan xu on 2020/12/11.
//

#ifndef ANDROIDGRAPHICSDEMOS_GJK_H
#define ANDROIDGRAPHICSDEMOS_GJK_H

#include "Convex.h"
#include "ext.hpp"
#include "glm.hpp"
#include <vector>
#include <memory>

namespace Phy {

    class RigidBody;

    namespace GJK {

        constexpr int MAX_EPA_ITER_TIMES = 64;
        constexpr float EPA_EPS = 0.0001f;

        class SupportPointInGJK
        {
        public:

            SupportPointInGJK() = default;
            SupportPointInGJK(const SupportPointInGJK &) = default;
            SupportPointInGJK(SupportPointInGJK &&) = default;
            SupportPointInGJK & operator= (const SupportPointInGJK &) = default;
            SupportPointInGJK(const std::shared_ptr<Convex>& c1, const std::shared_ptr<Convex>& c2, glm::vec3 dir);

            glm::vec3 minkowski_pos;
            glm::vec3 a_origin_wpos;
            glm::vec3 b_origin_wpos;
        };

        class FaceInGJK
        {
        public:
            FaceInGJK() = default;
            FaceInGJK(const FaceInGJK &) = default;
            FaceInGJK(FaceInGJK &&) = default;
            FaceInGJK & operator= (const FaceInGJK &) = default;
            FaceInGJK(SupportPointInGJK p1, SupportPointInGJK p2, SupportPointInGJK p3);

            SupportPointInGJK pos1;
            SupportPointInGJK pos2;
            SupportPointInGJK pos3;
            glm::vec3 norm;
        };

        class EdgeInGJK
        {
        public:
            EdgeInGJK() = default;
            EdgeInGJK(const EdgeInGJK &) = default;
            EdgeInGJK & operator= (const EdgeInGJK &) = default;
            EdgeInGJK(SupportPointInGJK p1, SupportPointInGJK p2);

            SupportPointInGJK pos1;
            SupportPointInGJK pos2;
            glm::vec3 mid_point;
        };

        class Simplex
        {
        public:

            const std::vector<SupportPointInGJK> & GetPoints();

            /*
             * 添加一个新的点
             * */
            void AddPoint(SupportPointInGJK p);

            /*
             *  目前方向是dir，更新Simplex，更新下一次迭代使用的方向，返回 原点 是否在当前的单纯型内
             * */
            bool UpdateSimplex(glm::vec3 & dir);

            bool UpdateLine(glm::vec3 & dir);

            bool UpdateTriangle(glm::vec3 & dir);

            bool UpdateTetrahedron(glm::vec3 & dir);

        private:
            std::vector<SupportPointInGJK> points;
        };

        class CollisionInfo
        {
        public:
            CollisionInfo() : collision(false) {}
            CollisionInfo(glm::vec3 pos1, glm::vec3 pos2, glm::vec3 norm) : collision(true), rb1_pos(pos1),  rb2_pos(pos2), normal(norm) {}
            bool collision;
            glm::vec3 rb1_pos;
            glm::vec3 rb2_pos;
            glm::vec3 normal;
        };

        bool GJKCollision(const std::shared_ptr<Phy::RigidBody>& r1, const std::shared_ptr<Phy::RigidBody>& r2, Simplex & gjk_simplex);
        CollisionInfo EPACollision(const std::shared_ptr<Phy::RigidBody>& r1, const std::shared_ptr<Phy::RigidBody>& r2);
    }
}
#endif //ANDROIDGRAPHICSDEMOS_GJK_H
